#include "R_to_Axial.h"

float inv_Sqrt(float x) {
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

void R_Matrix_to_Axial(Matrix_to_R_Typdef *R_out , Axial_Typdef *Axi)
{
    float cos_theta = 0.5f * (R_out->Matrix_to_R[0] + R_out->Matrix_to_R[4] + R_out->Matrix_to_R[8] - 1);

    float sin_theta = inv_Sqrt((R_out->Matrix_to_R[5] - R_out->Matrix_to_R[7]) * (R_out->Matrix_to_R[5] - R_out->Matrix_to_R[7]) +
                            (R_out->Matrix_to_R[6] - R_out->Matrix_to_R[2]) * (R_out->Matrix_to_R[6] - R_out->Matrix_to_R[2]) +
                            (R_out->Matrix_to_R[1] - R_out->Matrix_to_R[3]) * (R_out->Matrix_to_R[1] - R_out->Matrix_to_R[3])) / 2;

    Axi->Angle_0 = atan2f(sin_theta, cos_theta);

    if (sin_theta == 0) {
        // 旋转角为0，即无旋转
        Axi->Angle_0 = 0;
        return;
    }

    float x = (R_out->Matrix_to_R[7] - R_out->Matrix_to_R[5]) / (2 * sin_theta);
    float y = (R_out->Matrix_to_R[2] - R_out->Matrix_to_R[6]) / (2 * sin_theta);
    float z = (R_out->Matrix_to_R[3] - R_out->Matrix_to_R[1]) / (2 * sin_theta);

    Axi->long_l = x;
    Axi->long_m = y;
    Axi->long_n = z;
}